# coding: utf8

from numpy import array, matmul
from scipy import optimize as opt


def extract_rotation(source_cloud, target_cloud):
    """ Extract rotation limiting the Y axis.
    :param source_cloud: the source point cloud.
    :param target_cloud: the target point cloud.
    :return: the rotation matrix.
    """

    def func(p):
        r = array([
            [p[0], 0, p[1]],
            [0, 1, 0],
            [p[2], 0, p[3]]
        ])
        return sum([pow(matmul(r.reshape(3, 3), s) - t, 2).sum() for s, t in zip(source_cloud, target_cloud)])

    rs = opt.fmin_powell(
            func,
            [1, 0, 0, 1],
            disp=False
    )
    return array([
        [rs[0], 0, rs[1]],
        [0, 1, 0],
        [rs[2], 0, rs[3]]
    ])
